//
//  Line.c
//  Vector_Math
//
//  Created by John R Peterson on 3/11/12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#include <stdio.h>
#include "Line.h"

// internal functions that noone else should use
void Line_accumulate_INTERNAL(Line *this, Vec *v);
void Line_wrapUp_INTERNAL(Line *this);
void Line_unnormalize_INTERNAL(Line* this);
// internal functions
//void PolyLine_updatePos_INTERNAL(PolyLine *this, Vec *pos);

// only works for 2D vectors but would be expensive to check for this
// also be aware that Line will edit the vec_list given to it
Line* Line_init(VecList *P) {
    if (P->size < 2) {
        printf("Warning: need atleast 2 points to construct a line: Line_init\n");
        exit(1);
    }
    
    // allocate space for the line itself
    Line* this = (Line *) malloc(sizeof(Line));
    if (this == NULL) {
        printf("Warning: malloc failed: Line_init:1 \n");
        exit(1);
    }
    
    // initialize zero matricies and vectors
    // TODO may not need to do all of these here!
    this->length = -1;  // haven't computed length yet and won't unless we need to
    this->mode = TRUE;
    this->centroid = Vec_createZeros(2);
    this->Sigma = Matrix_createZeros(2,2);
    this->points = VecList_createS((P->size) * 2); // initialize empty list
    
    // for each point
    int i;
    for (i = 0; i < P->size; i++) {
        Line_accumulate_INTERNAL(this, P->list[i]);
    }
    // wrap up
    Line_wrapUp_INTERNAL(this);
    
    return this;
}

// only works for 2D vectors but would be expensive to check for this
// also be aware that Line will edit the vec_list given to it
Line* Line_2Point(Vec *a, Vec *b) {
    
    // allocate space for the line itself
    Line* this = (Line *) malloc(sizeof(Line));
    if (this == NULL) {
        printf("Warning: malloc failed: Line_init:1 \n");
        exit(1);
    }
    
    // initialize zero matricies and vectors
    // TODO may not need to do all of these here!
    this->centroid = Vec_createZeros(2);
    this->Sigma = Matrix_createZeros(2,2);
    this->points = VecList_createS(4); // initialize empty list
    
    // for each point
    Line_accumulate_INTERNAL(this, a);
    Line_accumulate_INTERNAL(this, b);
   
    // wrap up
    Line_wrapUp_INTERNAL(this);
    
    // easy to compute length for this line but this equation is not 
    // strictly true for lines with more points
    this->length = 2*sqrt(this->Lambda->val[0]);
    return this;
}

// manually create a line with these parameters doesn't copy just uses these fields
Line* Line_createMan(Vec *centroid, Vec *direction, Vec *lambda) {
    // allocate space for the line itself
    Line* this = (Line *) malloc(sizeof(Line));
    if (this == NULL) {
        printf("Warning: malloc failed: Line_init:1 \n");
        exit(1);
    }
    
    // set our parameters
    this->centroid = centroid;
    this->direction = direction;
    this->Lambda = lambda;
    this->mode = FALSE;
    
    return this;
}

// add a single point to the line
void Line_add(Line *this, Vec *v) {
    if (this->mode) {
        Line_unnormalize_INTERNAL(this);
        Line_accumulate_INTERNAL(this, v);
        Line_wrapUp_INTERNAL(this);
    } else {
        printf("Warning: this is a manual line which does not allow add!\n");
        exit(1);
    }
}

void Line_addMany(Line *this, VecList *vlist) {
    if (this->mode) {
        Line_unnormalize_INTERNAL(this);
        int i;
        for (i = 0; i < vlist->size; i++) {
            Line_accumulate_INTERNAL(this, vlist->list[i]);
        }
        Line_wrapUp_INTERNAL(this);
    } else {
        printf("Warning: this is a manual line which does not allow addMany!\n");
        exit(1);
    }
}

// destroys a line
void Line_destroy(Line *this) {
    
    // destroy all the matricies
    Matrix_destroy(this->Sigma);
    Vec_destroy(this->centroid);
    Vec_destroy(this->direction);
    Vec_destroy(this->Lambda);
    
    // destroy the Vec_list
    if (this->mode) {
        VecList_destroy(this->points);
    }
    
    // destroy the structure itself
    free(this);
}

// destroys a line and all the points that it points to
// use with caution
void Line_destroyR(Line *this) {
    
    // destroy all the matricies
    printf("destroying stuff\n");
    Matrix_destroy(this->Sigma);
    Vec_destroy(this->centroid);
    Vec_destroy(this->direction);
    Vec_destroy(this->Lambda);
    
    printf("done destroying stuff\n");
    
    // destroy the Vec_list
    if (this->mode) {
        VecList_destroyR(this->points);
    }
    
    // destroy the structure itself
    free(this);
}

// computes perpendicular distance from line to the point
float Line_distance(Line *this, Vec *p) {
    if ((p->size != 2) && (p->size != 3)) {
        printf("Warning only supports 2D or 2D homogeneous vectors: Line_distance\n");
        exit(1);
    }
    return Vec_L2normD(Line_perpendicular(this, p));
}

// returns shortest vector between the position p and the line l
Vec* Line_perpendicular(Line *this, Vec *p) {
    if ((p->size != 2) && (p->size != 3)) {
        printf("Warning only supports 2D or 2D homogeneous vectors: Line_distance\n");
        exit(1);
    }
    return Vec_subtractDD(Vec_subtract(this->centroid, p), Vec_scale(this->direction, Vec_dotProductD(Vec_subtract(this->centroid, p), this->direction)));
}

// computes parallel distace along line from centroid to point
float Line_distanceP(Line *this, Vec *p) {
    if ((p->size != 2) && (p->size != 3)) {
        printf("Warning only supports 2D or 2D homogeneous vectors: Line_distanceP\n");
        exit(1);
    }
    return fabsf(Vec_dotProductD(Vec_subtract(this->centroid,p), this->direction));
}

// interperets major axis as a line between two points and returns its length
// not strictly correct for lines that contain more than two points
// will likely overestimate the length of the line, since we are looking for the most extreme point
float Line_length(Line *this) {
    // if we haven't computed the length yet
    if (this->length < 0) {
        int i;
        float maxd = 0;
        float d;
        for (i = 0; i < this->points->size; i++) {
            d = Vec_L2normD(Vec_subtract(this->centroid, this->points->list[i]));
            if (d > maxd) {
                maxd = d;
            }
        }
        this->length = 2*maxd;
    } 
    return this->length;
    
}

// for internal use only!
void Line_accumulate_INTERNAL(Line* this, Vec *v) {
    // accumulate for centroid comutation
    if (!(v->isColumn)) {
        v = Vec_transpose(v);
    }
    Vec_addI(this->centroid, v);
    Matrix_addID(this->Sigma, Vec_multiply(v,Vec_transpose(v)));
    VecList_add(this->points,v);
}

void Line_print(Line *this) {
    printf("centroid at: ");
    Vec_print(this->centroid);
    printf("direction: ");
    Vec_print(this->direction);
    printf("estimated line length: %f\n",Line_length(this));
}

// for internal use only!!!
void Line_wrapUp_INTERNAL(Line *this) {
    float sf = 1.0/(this->points->size);
    
    // compute expected values
    Vec_scaleI(this->centroid,sf);
    Matrix_scaleI(this->Sigma,sf);
    
    // compute sigma
    Matrix_subtractID(this->Sigma, Vec_multiply_D(this->centroid,Vec_transpose(this->centroid)));
    
    // use dominant eigne vector of sigma
    Matrix* ans[2];
    Matrix_eigDecomp(this->Sigma, ans);
    // record our eigenvalues in a vector
    float eigV[] = {ans[0]->val[0][0],ans[0]->val[1][1]};
    this->Lambda = Vec_create(eigV, 2);
    Matrix_destroy(ans[0]);
    // record the dominant eigen vector
    this->direction = Matrix_getColumn(ans[1],0); // 1st column contains dominant eigenvector
    Matrix_destroy(ans[1]);
    
}

// for internal use only!!
void Line_unnormalize_INTERNAL(Line *this) {
    float sf = (float) this->points->size;
    Vec_scaleI(this->centroid,sf);
    Matrix_scaleI(this->Sigma,sf);
}

/***************************************************************/

void LineList_add(LineList *llist, Line *l) {
    // if still room to spare
    if (llist->size < llist->maxsize) {
        llist->list[llist->size] = l;
        (llist->size)++;
    } 
    // need to expand the list
    else {
        llist->list = (Line **) realloc(llist->list,sizeof(Line *)*(llist->maxsize) * 2);
        if (llist->list == NULL) {
            printf("Warning: realloc failed: Vec_list_add\n");
            exit(1);
        }
        llist->maxsize = (llist->maxsize)*2;
        // add the new point
        llist->list[llist->size] = l;
        (llist->size)++;
    }
}

// copies the structure of the list, but doesn't make copies of the contained verticies
LineList* LineList_copy(LineList *that) {
    LineList* this = LineList_createS((that->size)*2);
    
    memcpy(this->list,that->list,(sizeof(Line *) * (that->size)));
    
    this->size = that->size;
    return this;
}

LineList* LineList_create(void) {
    return LineList_createS(BASESIZE);
}

LineList* LineList_createS(int initSize) {
    if (initSize <= 0) {
        printf("Warning: invalid initial size, setting to default: Vec_list_createS\n");
        initSize = BASESIZE;
    }
    LineList *llist = (LineList *) malloc(sizeof(LineList));
    // allocate size for the structure
    if (llist == NULL) {
        printf("Warning: malloc failed: LineList_createS:1\n");
        exit(1);
    }
    
    llist->size = 0;
    llist->maxsize = initSize;
    
    // allocate space for its list of vector pointers
    llist->list = (Line **) malloc(sizeof(Line *) * initSize);
    if (llist->list == NULL) {
        printf("Warning: malloc failed: LineList_createS:2\n");
        exit(1);
    }
    return llist;
}

// destroys only this structure
void LineList_destroy(LineList *llist) {
    free(llist->list);
    free(llist);
}

// destroys every line in the line list, but doesn't touch the vectors the lines are made of
void LineList_destroyR(LineList *llist) {
    int i;
    for (i = 0; i < llist->size; i++) {
        Line_destroy(llist->list[i]);
    }
}

// destroys all contained vectors and this structure and destroys the points those lines are made of
void LineList_destroyRR(LineList *llist) {
    int i;
    for (i = 0; i < llist->size; i++) {
        Line_destroyR(llist->list[0]);
    }
    free(llist->list);
    free(llist);
}

// removes Line from Line list without deleting it
short LineList_remove(LineList *llist, int index) {
    if ((index >= llist->size) || (index < 0)) {
        return FALSE;
    } 
    // if its on the end, we can get rid of it really quick
    else if (index == ((llist->size) - 1)) {
        (llist->size)--;
    } 
    // otherwise shift everthing over to the left
    else {
        memcpy(&(llist->list[index]),&(llist->list[index + 1]),sizeof(Line *) * ((llist->size) - (index + 1)));
        (llist->size)--;
    }
    return TRUE;
}

// removes Line from list and deletes the line
short LineList_removeR(LineList *llist, int index) {
    if ((index >= llist->size) || (index < 0)) {
        return FALSE;
    } 
    // if its on the end, we can get rid of it really quick
    else if (index == ((llist->size) - 1)) {
        Line_destroy(llist->list[index]);
        (llist->size)--;
    } 
    // otherwise shift everthing over to the left
    else {
        Line_destroy(llist->list[index]);
        memcpy(llist->list[index],llist->list[index + 1],sizeof(Line *) * ((llist->size) - 1 - (index + 1)));
    }
    return TRUE;
}

/****************************************************************/


PolyLine* PolyLine_init(VecList *verticies) {
    
    // allocate room for the structure
    PolyLine *pL = (PolyLine *) malloc(sizeof(PolyLine));
    if (pL == NULL) {
        printf("Warning: malloc failed: PolyLine_init:1\n");
        exit(1);
    }
    pL->vertices = VecList_copy(verticies); // make a copy of it so we can destroy without worry
    
    // create Line list
    pL->Llist = LineList_createS((verticies->size - 1) * 2);
    
    // create lines and add to the list
    int i;
    for (i = 1; i < verticies->size; i++) {
        LineList_add(pL->Llist, Line_2Point(verticies->list[i-1],verticies->list[i]));
    }
    
    // assume that we start at the begining of the polyline
    pL->pos = 0;
    
    return pL;
}

// appends an extra vertex to the end of the poly line and 
void PolyLine_addVertexEnd(PolyLine *this, Vec *v) {
   
    // create line between last vertex and new vertex and add to list
    LineList_add(this->Llist,Line_2Point(this->vertices->list[this->vertices->size - 1], v));
    
    // add point to list
    VecList_add(this->vertices,v);
}

// doesn't destroy the underlying verticies
void PolyLine_destroy(PolyLine *this) {
    
    // first destroy every line
    printf("destroying everyline\n");
    LineList_destroyR(this->Llist); // want to destroy the lines but not the verticies
    
    printf("destroying vertex list\n");
    // now destroy the vertex list
    VecList_destroy(this->vertices);
    
    // now destroy the whole structure
    printf("destroying this\n");
    free(this);
}


// computes perpendicular distance from the position vector to the polyline
// the lines on either end are considered to be infinitely long, so will return erros to those
// when it leaves the line
/*float PolyLine_error(PolyLine *this, Vec *pos) {
    // update the polyline to reflect the closest segment
    PolyLine_updatePos_INTERNAL(this, pos);
    
    // use the closest segment to compute an error
    return Line_distance(this->Llist[this->pos], pos);
}*/

// uses an equidistant dividing line between segments to transition between them
// will cause the robot to cut the corner
float PolyLine_error(PolyLine *this, Vec *pos) {
    // compute error to current line
    float curerror = Line_distance(this->Llist->list[this->pos], pos);
    if (this->pos != (this->Llist->size -1)) {
        // if the error to the next line segment is smaller, then we switch over to it
        float nexterror = Line_distance(this->Llist->list[(this->pos) + 1], pos);
        if (nexterror <= curerror) {
            (this->pos)++;
            return nexterror;
        } 
    } 
    return curerror;
}

// removes a vertex from the beginning of the polyline and destroys the line
// does not destroy the underlying vertex, just removes it from the list
void PolyLine_removeVertexStart(PolyLine *this) {
    
    // destroy the line
    LineList_removeR(this->Llist,0);
    
    // remove the vertex
    VecList_remove(this->vertices,0);
  
    // decrement position to compensate unless already at the beginning
    if (this->pos > 0) {
        (this->pos)--;
    } else {
        this->pos = 0; // set it just to be safe
    }
}

// removes a vertex from the beginning of the polyline and destroys the line
// destroys the vertex as well as removes it from the list
void PolyLine_removeVertexRStart(PolyLine *this) {
    
    // destroy the line
    LineList_removeR(this->Llist,0);
    
    // remove the vertex and destroy it
    VecList_removeR(this->vertices,0);
    
    // decrement position to compensate unless already at the beginning
    if (this->pos > 0) {
        (this->pos)--;
    } else {
        this->pos = 0; // set it just to be safe
    }
}

// given the current position, and the poly line, figure out which line we should measure our error to
// using nearest neighbor association to their centroids, only examines a subset of all lines in the polyline
// TODO a sensible transfer between line segements!!!
// currently not used!
/*
void PolyLine_updatePos_INTERNAL(PolyLine *this, Vec *pos) {
    
    float dprev,dcur,dnext;
    int cur = this->pos;
    // compute distance to the centroid of the current, previous, and next line segments, 
    // and pick the closest one
    if ((this->pos > 0) && this->pos < (this->size -1)) {
        // previous distance
        dprev = Vec_L2norm2(Vec_subtract(this->Llist[cur - 1]->centroid,pos));
        // current distance
        dcur = Vec_L2norm2(Vec_subtract(this->Llist[cur]->centroid,pos));
        // next distance
        dnext = Vec_L2norm2(Vec_subtract(this->Llist[cur+1]->centroid,pos));
        
        if ((dprev < dcur) && (dprev < dnext)) {
            this->pos = cur-1;
        } else if ((dnext < dprev) && (dnext < dcur)) {
            this->pos = cur+1;
        }
        // and don't update otherwise since current is just fine
    } else if (this->pos == 0) {
        // current distance
        dcur = Vec_L2norm2(Vec_subtract(this->Llist[cur]->centroid,pos));
        // next distance
        dnext = Vec_L2norm2(Vec_subtract(this->Llist[cur+1]->centroid,pos));
        
        if (dnext < dcur) {
            this->pos = cur+1;
        }

    } else if (this->pos == (this->size -1)) {
        // previous distance
        dprev = Vec_L2norm2(Vec_subtract(this->Llist[cur - 1]->centroid,pos));
        // current distance
        dcur = Vec_L2norm2(Vec_subtract(this->Llist[cur]->centroid,pos));
        
        if (dprev < dcur) {
            this->pos = cur-1;
        }
    } else {
        printf("Warning invalid position\n");
    }
}*/




